﻿using MauiControl.Models;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;

namespace MauiControl.Services
{
    public class BluetoothService
    {
        private readonly IBluetoothService _platformService;
        private List<byte> _receivedDataBuffer = new List<byte>();
        private const int ExpectedDataLength = 73; // 预期的完整数据包长度（根据设备协议调整）

        public BluetoothService(IBluetoothService platformService)
        {
            _platformService = platformService;

            // 订阅 DataReceived 事件
            _platformService.DataReceived += OnDataReceived;
        }

        /// <summary>
        /// 搜索蓝牙设备
        /// </summary>
        public async Task<List<BleDeviceInfo>> SearchDevicesAsync(CancellationToken cancellationToken, Action<BleDeviceInfo> onDeviceAddedCallback)
        {
            return await _platformService.SearchDevicesAsync(cancellationToken, onDeviceAddedCallback);
        }

        /// <summary>
        /// 连接蓝牙设备
        /// </summary>
        public async Task<BleDeviceInfo> ConnectDeviceAsync(string deviceId)
        {
            var deviceInfo = await _platformService.ConnectDeviceAsync(deviceId);
            await SendDeviceInitializationCommandsAsync(); // 发送设备初始化指令
            return deviceInfo;
        }

        /// <summary>
        /// 发送设备初始化指令
        /// </summary>
        private async Task SendDeviceInitializationCommandsAsync()
        {
            try
            {
                // 发送保持连接指令 (0x29)
                Debug.WriteLine("准备发送保持连接指令 (0x29)...");
                await _platformService.WriteDataAsync(new byte[] { 0x29 });
                Debug.WriteLine("保持连接指令 (0x29) 发送成功。");

                // 等待设备的响应（如果有）
                await Task.Delay(500); // 等待 500ms，确保设备有足够的时间响应

                // 读取设备的响应数据（如果有）
                var response = await _platformService.ReadRawDataAsync();
                if (response != null && response.Length > 0)
                {
                    Debug.WriteLine($"接收到设备的响应: {BitConverter.ToString(response)}");
                }
                else
                {
                    Debug.WriteLine("未接收到设备的响应。");
                }

                // 发送其他初始化指令
                Debug.WriteLine("准备发送安装指令...");
                await _platformService.WriteDataAsync(new byte[] { 0x20, 0x00, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 });
                Debug.WriteLine("安装指令发送成功。");

                Debug.WriteLine("准备设置加速度和陀螺仪量程...");
                await _platformService.WriteDataAsync(new byte[] { 0x33, 0x03, 0x02 });
                Debug.WriteLine("加速度和陀螺仪量程设置指令发送成功。");

                // 设置功能订阅标识
                ushort Cmd_ReportTag = 0x0065; // 0x0065 = 0x0001 | 0x0004 | 0x0020 | 0x0040
                byte[] commandParams = new byte[11]; // 将变量名从 params 改为 commandParams
                commandParams[0] = 0x12; // 功能标签
                commandParams[1] = 5;    // 静止状态加速度阀值
                commandParams[2] = 255;  // 静止归零速度(单位cm/s) 0:不归零 255:立即归零
                commandParams[3] = 0;    // 动态归零速度(单位cm/s) 0:不归零
                commandParams[4] = 0;    // 气压计滤波和磁场融合设置
                commandParams[5] = 60;   // 数据主动上报的传输帧率[取值0-250HZ], 0表示0.5HZ
                commandParams[6] = 1;    // 陀螺仪滤波系数[取值0-2],数值越大越平稳但实时性越差
                commandParams[7] = 3;    // 加速计滤波系数[取值0-4],数值越大越平稳但实时性越差
                commandParams[8] = 5;    // 磁力计滤波系数[取值0-9],数值越大越平稳但实时性越差
                commandParams[9] = (byte)(Cmd_ReportTag & 0xff); // 低字节
                commandParams[10] = (byte)((Cmd_ReportTag >> 8) & 0xff); // 高字节

                // 发送订阅指令
                Debug.WriteLine("准备发送订阅指令...");
                await _platformService.WriteDataAsync(commandParams); // 传递正确的参数
                Debug.WriteLine("订阅指令发送成功。");

                //// 订阅四元数、欧拉角、角速度和加速度数据
                //Debug.WriteLine("准备发送订阅指令...");
                //await _platformService.WriteDataAsync(new byte[] { 0x12, 0x01, 0x65 }); // 0x65 = 0x01 | 0x04 | 0x20 | 0x40
                //Debug.WriteLine("订阅指令发送成功。");
            }
            catch (Exception ex)
            {
                Debug.WriteLine($"发送初始化指令时出错: {ex.Message}");
            }
        }

        /// <summary>
        /// 读取传感器数据
        /// </summary>
        public async Task<SensorData> ReadSensorDataAsync()
        {
            var rawData = await _platformService.ReadRawDataAsync();
            return ParseSensorData(rawData);
        }

        /// <summary>
        /// 断开连接
        /// </summary>
        public void Disconnect()
        {
            _platformService.Disconnect();
        }

        /// <summary>
        /// 获取电池电量
        /// </summary>
        public async Task<string> GetBatteryLevelAsync(string deviceId)
        {
            var batteryLevel = await _platformService.GetBatteryLevelAsync(deviceId);
            return batteryLevel; // 可以在这里对电量值进行进一步处理
        }

        /// <summary>
        /// 解析传感器数据
        /// </summary>
        private SensorData ParseSensorData(byte[] rawData)
        {
            const float scaleAccel = 0.00478515625f;
            const float scaleAngle = 0.0054931640625f;
            const float scaleAngleSpeed = 0.06103515625f;
            const float scaleMag = 0.15106201171875f;
            const float scaleTemperature = 0.01f;
            const float scaleAirPressure = 0.0002384185791f;
            const float scaleHeight = 0.0010728836f;
            const float scaleQuat = 0.000030517578125f;

            var sensorData = new SensorData();

            // 检查数据是否为空
            if (rawData == null || rawData.Length < 7) // 至少需要7个字节（包头和ctl）
            {
                return sensorData; // 返回空数据
            }

            // 检查数据包头
            if (rawData[0] != 0x11)
            {
                return sensorData; // 返回空数据
            }

            // 解析控制标志位
            ushort ctl = (ushort)((rawData[2] << 8) | rawData[1]);
            Debug.WriteLine($"Control Flags: {ctl:X4}"); // 输出 ctl 的值

            int l = 7; // 跳过前7个字节（包头和ctl）

            // 根据 ctl 的标志位动态解析数据
            if ((ctl & 0x0001) != 0 && l + 6 <= rawData.Length) // 无重力加速度
            {
                sensorData.AccelX = BitConverter.ToInt16(rawData, l) * scaleAccel; l += 2;
                sensorData.AccelY = BitConverter.ToInt16(rawData, l) * scaleAccel; l += 2;
                sensorData.AccelZ = BitConverter.ToInt16(rawData, l) * scaleAccel; l += 2;

                Debug.WriteLine($"解析无重力加速度数据: X={sensorData.AccelX}, Y={sensorData.AccelY}, Z={sensorData.AccelZ}");
            }

            if ((ctl & 0x0002) != 0 && l + 6 <= rawData.Length) // 含重力加速度
            {
                // 解析含重力加速度数据
                float accelXWithGravity = BitConverter.ToInt16(rawData, l) * scaleAccel; l += 2;
                float accelYWithGravity = BitConverter.ToInt16(rawData, l) * scaleAccel; l += 2;
                float accelZWithGravity = BitConverter.ToInt16(rawData, l) * scaleAccel; l += 2;
            }

            if ((ctl & 0x0004) != 0 && l + 6 <= rawData.Length) // 角速度
            {
                sensorData.AngularVelX = BitConverter.ToInt16(rawData, l) * scaleAngleSpeed; l += 2;
                sensorData.AngularVelY = BitConverter.ToInt16(rawData, l) * scaleAngleSpeed; l += 2;
                sensorData.AngularVelZ = BitConverter.ToInt16(rawData, l) * scaleAngleSpeed; l += 2;
            }

            if ((ctl & 0x0008) != 0 && l + 6 <= rawData.Length) // 磁场强度
            {
                float magX = BitConverter.ToInt16(rawData, l) * scaleMag; l += 2;
                float magY = BitConverter.ToInt16(rawData, l) * scaleMag; l += 2;
                float magZ = BitConverter.ToInt16(rawData, l) * scaleMag; l += 2;
            }

            if ((ctl & 0x0010) != 0 && l + 8 <= rawData.Length) // 温度、气压、高度
            {
                float temperature = BitConverter.ToInt16(rawData, l) * scaleTemperature; l += 2;
                int pressure = BitConverter.ToInt32(rawData, l) & 0x00FFFFFF; l += 3;
                int height = BitConverter.ToInt32(rawData, l) & 0x00FFFFFF; l += 3;
            }

            if ((ctl & 0x0020) != 0 && l + 8 <= rawData.Length) // 四元数
            {
                sensorData.QuaternionW = BitConverter.ToInt16(rawData, l) * scaleQuat; l += 2;
                sensorData.QuaternionX = BitConverter.ToInt16(rawData, l) * scaleQuat; l += 2;
                sensorData.QuaternionY = BitConverter.ToInt16(rawData, l) * scaleQuat; l += 2;
                sensorData.QuaternionZ = BitConverter.ToInt16(rawData, l) * scaleQuat; l += 2;
            }

            if ((ctl & 0x0040) != 0 && l + 6 <= rawData.Length) // 角度
            {
                sensorData.EulerX = BitConverter.ToInt16(rawData, l) * scaleAngle; l += 2;
                sensorData.EulerY = BitConverter.ToInt16(rawData, l) * scaleAngle; l += 2;
                sensorData.EulerZ = BitConverter.ToInt16(rawData, l) * scaleAngle; l += 2;
            }

            return sensorData;
        }

        /// <summary>
        /// 处理接收到的数据
        /// </summary>
        private void OnDataReceived(object sender, byte[] data)
        {
            try
            {
                Debug.WriteLine($"接收到数据包: {BitConverter.ToString(data)}");

                // 将接收到的数据添加到缓冲区
                _receivedDataBuffer.AddRange(data);

                // 检查缓冲区中的数据是否包含完整的数据包
                while (_receivedDataBuffer.Count >= ExpectedDataLength)
                {
                    // 查找数据包头 (0x11)
                    int packetStartIndex = _receivedDataBuffer.IndexOf(0x11);
                    if (packetStartIndex == -1)
                    {
                        // 如果没有找到包头，清空缓冲区
                        Debug.WriteLine("未找到数据包头，清空缓冲区。");
                        _receivedDataBuffer.Clear();
                        return;
                    }

                    // 移除包头之前的数据
                    if (packetStartIndex > 0)
                    {
                        Debug.WriteLine($"移除无效数据: {BitConverter.ToString(_receivedDataBuffer.Take(packetStartIndex).ToArray())}");
                        _receivedDataBuffer.RemoveRange(0, packetStartIndex);
                    }

                    // 检查剩余数据是否足够一个完整的数据包
                    if (_receivedDataBuffer.Count < ExpectedDataLength)
                    {
                        return;
                    }

                    // 提取完整的数据包
                    byte[] completeData = _receivedDataBuffer.Take(ExpectedDataLength).ToArray();

                    // 处理完整的数据包
                    Debug.WriteLine($"接收到完整数据包: {BitConverter.ToString(completeData)}");
                    ProcessCompleteData(completeData);

                    // 移除已处理的数据
                    _receivedDataBuffer.RemoveRange(0, ExpectedDataLength);
                }
            }
            catch (Exception ex)
            {
                Debug.WriteLine($"处理接收到的数据时出错: {ex.Message}");
            }
        }

        /// <summary>
        /// 处理完整数据包
        /// </summary>
        private void ProcessCompleteData(byte[] completeData)
        {
            try
            {
                // 检查数据是否为空
                if (completeData == null || completeData.Length < 7) // 至少需要7个字节（包头和ctl）
                {
                    Debug.WriteLine("数据包为空或长度不足。");
                    return;
                }

                // 检查数据包头
                if (completeData[0] != 0x11)
                {
                    Debug.WriteLine("数据包头不正确。");
                    return;
                }

                // 解析控制标志位
                ushort ctl = (ushort)((completeData[2] << 8) | completeData[1]);
                Debug.WriteLine($"Control Flags: {ctl:X4}"); // 输出 ctl 的值

                // 检查 ctl 的标志位
                if ((ctl & 0x0001) != 0)
                {
                    Debug.WriteLine("数据包包含无重力加速度数据。");
                }
                if ((ctl & 0x0002) != 0)
                {
                    Debug.WriteLine("数据包包含含重力加速度数据。");
                }
                if ((ctl & 0x0004) != 0)
                {
                    Debug.WriteLine("数据包包含角速度数据。");
                }
                if ((ctl & 0x0008) != 0)
                {
                    Debug.WriteLine("数据包包含磁场数据。");
                }
                if ((ctl & 0x0010) != 0)
                {
                    Debug.WriteLine("数据包包含气压计数据。");
                }
                if ((ctl & 0x0020) != 0)
                {
                    Debug.WriteLine("数据包包含四元数数据。");
                }
                if ((ctl & 0x0040) != 0)
                {
                    Debug.WriteLine("数据包包含欧拉角数据。");
                }

                int l = 7; // 跳过前7个字节（包头和ctl）

                // 根据 ctl 的标志位动态解析数据
                if ((ctl & 0x0001) != 0 && l + 6 <= completeData.Length) // 无重力的加速度
                {
                    float accelX = BitConverter.ToInt16(completeData, l) * 0.00478515625f; l += 2;
                    float accelY = BitConverter.ToInt16(completeData, l) * 0.00478515625f; l += 2;
                    float accelZ = BitConverter.ToInt16(completeData, l) * 0.00478515625f; l += 2;
                    Debug.WriteLine($"解析无重力加速度数据: X={accelX}, Y={accelY}, Z={accelZ}");
                }

                if ((ctl & 0x0002) != 0 && l + 6 <= completeData.Length) // 含重力的加速度
                {
                    float accelXWithGravity = BitConverter.ToInt16(completeData, l) * 0.00478515625f; l += 2;
                    float accelYWithGravity = BitConverter.ToInt16(completeData, l) * 0.00478515625f; l += 2;
                    float accelZWithGravity = BitConverter.ToInt16(completeData, l) * 0.00478515625f; l += 2;
                    Debug.WriteLine($"解析含重力加速度数据: X={accelXWithGravity}, Y={accelYWithGravity}, Z={accelZWithGravity}");
                }

                if ((ctl & 0x0004) != 0 && l + 6 <= completeData.Length) // 角速度
                {
                    float angularVelX = BitConverter.ToInt16(completeData, l) * 0.06103515625f; l += 2;
                    float angularVelY = BitConverter.ToInt16(completeData, l) * 0.06103515625f; l += 2;
                    float angularVelZ = BitConverter.ToInt16(completeData, l) * 0.06103515625f; l += 2;
                    Debug.WriteLine($"解析角速度数据: X={angularVelX}, Y={angularVelY}, Z={angularVelZ}");
                }

                if ((ctl & 0x0008) != 0 && l + 6 <= completeData.Length) // 磁场数据
                {
                    float magX = BitConverter.ToInt16(completeData, l) * 0.15106201171875f; l += 2;
                    float magY = BitConverter.ToInt16(completeData, l) * 0.15106201171875f; l += 2;
                    float magZ = BitConverter.ToInt16(completeData, l) * 0.15106201171875f; l += 2;
                    Debug.WriteLine($"解析磁场数据: X={magX}, Y={magY}, Z={magZ}");
                }

                if ((ctl & 0x0010) != 0 && l + 8 <= completeData.Length) // 气压计数据
                {
                    float temperature = BitConverter.ToInt16(completeData, l) * 0.01f; l += 2;
                    int pressure = BitConverter.ToInt32(completeData, l) & 0x00FFFFFF; l += 3;
                    int height = BitConverter.ToInt32(completeData, l) & 0x00FFFFFF; l += 3;
                    Debug.WriteLine($"解析气压计数据: 温度={temperature}, 气压={pressure}, 高度={height}");
                }

                if ((ctl & 0x0020) != 0 && l + 8 <= completeData.Length) // 四元数
                {
                    float quaternionW = BitConverter.ToInt16(completeData, l) * 0.000030517578125f; l += 2;
                    float quaternionX = BitConverter.ToInt16(completeData, l) * 0.000030517578125f; l += 2;
                    float quaternionY = BitConverter.ToInt16(completeData, l) * 0.000030517578125f; l += 2;
                    float quaternionZ = BitConverter.ToInt16(completeData, l) * 0.000030517578125f; l += 2;
                    Debug.WriteLine($"解析四元数数据: W={quaternionW}, X={quaternionX}, Y={quaternionY}, Z={quaternionZ}");
                }

                if ((ctl & 0x0040) != 0 && l + 6 <= completeData.Length) // 欧拉角
                {
                    float eulerX = BitConverter.ToInt16(completeData, l) * 0.0054931640625f; l += 2;
                    float eulerY = BitConverter.ToInt16(completeData, l) * 0.0054931640625f; l += 2;
                    float eulerZ = BitConverter.ToInt16(completeData, l) * 0.0054931640625f; l += 2;
                    Debug.WriteLine($"解析欧拉角数据: X={eulerX}, Y={eulerY}, Z={eulerZ}");
                }
            }
            catch (Exception ex)
            {
                Debug.WriteLine($"解析完整数据包时出错: {ex.Message}");
            }
        }
    }
}